/*该类的功能主要是点云的二维栅格处理
 edited by goldqiu -2022-04-9
*/
#include "map_conversion/pointcloud_process/pointcloud_2d_process.hpp"
#include "glog/logging.h"

namespace map_conversion {
Pointcloud2dProcess::Pointcloud2dProcess(YAML::Node& config_node) {

  _cloud_margin = config_node["inflation_radius"].as<double>();

  _inv_resolution = 1.0 / _resolution;
  int _max_x_id = (int)(_x_size * _inv_resolution);
  int _max_y_id = (int)(_y_size * _inv_resolution);
  int _max_z_id = (int)(_z_size * _inv_resolution);

   Global_map_file = config_node["Global_file_directory"].as<std::string>();
   local_cloud_topic = config_node["local_cloud_frame"].as<std::string>();
   global_z_adjust = atof((config_node["global_z"].as<std::string>()).c_str());
   local_z_adjust = atof((config_node["local_z"].as<std::string>()).c_str());
   cout << "max_angle:" << local_z_adjust << endl;
}

void Pointcloud2dProcess::find_Z_value(CloudData& cloud_data){
    std::cout << "初始点云数据点数：" << cloud_data.cloud_ptr->points.size() << std::endl;
    for(int i = 0; i < cloud_data.cloud_ptr->points.size() - 1; i++){
        cloud_data.cloud_ptr->points[i].z = cloud_data.cloud_ptr->points[i].z +0.7;
        }
    for(int i = 0; i < cloud_data.cloud_ptr->points.size() - 1; i++){
        if(cloud_data.cloud_ptr->points[i].z>max_z){
            max_z=cloud_data.cloud_ptr->points[i].z;
          }
        if(cloud_data.cloud_ptr->points[i].z<min_z){
            min_z=cloud_data.cloud_ptr->points[i].z;
          }
    }
    std::cout<<"orig max_z="<<max_z<<",min_z="<<min_z<<std::endl;
}

int Pointcloud2dProcess::global_map_init(void)
{
    if (pcl::io::loadPCDFile(Global_map_file, *(global_map_data.cloud_ptr)) == -1)
  {
        PCL_ERROR ("Couldn't read file: %s \n", Global_map_file.c_str());
        return (-1);
  } 
    find_Z_value(global_map_data);
    PassThroughFilter(global_map_data,global_map_after_filter,false);
}

void Pointcloud2dProcess::PassThroughFilter(CloudData& pcd_cloud,CloudData& cloud_after_PassThrough, const bool &flag_in)
{
    /*方法一：直通滤波器对点云进行处理。*/
    pcl::PassThrough<pcl::PointXYZ> passthrough;
    passthrough.setInputCloud(pcd_cloud.cloud_ptr);//输入点云
    passthrough.setFilterFieldName("z");//对z轴进行操作
    passthrough.setFilterLimits(min_z+local_z_adjust+global_z_adjust, max_z);//设置直通滤波器操作范围
    passthrough.setFilterLimitsNegative(flag_in);//true表示保留范围外，false表示保留范围内
    passthrough.filter(*cloud_after_PassThrough.cloud_ptr);//执行滤波，过滤结果保存在 cloud_after_PassThrough
    std::cout << "直通滤波后点云数据点数：" << cloud_after_PassThrough.cloud_ptr->points.size() << std::endl;
    for(int i = 0; i < cloud_after_PassThrough.cloud_ptr->points.size() - 1; i++){
     if(cloud_after_PassThrough.cloud_ptr->points[i].z>max_z){
       max_z=cloud_after_PassThrough.cloud_ptr->points[i].z;
     }
     if(cloud_after_PassThrough.cloud_ptr->points[i].z<min_z){
       min_z=cloud_after_PassThrough.cloud_ptr->points[i].z;
     }
     }
    std::cout<<"aft pass through filter: max_z="<<max_z<<",min_z="<<min_z<<std::endl;
}

void Pointcloud2dProcess::global_map_process(const CloudData& _cloud,nav_msgs::OccupancyGrid& msg,const double& x_min,const double& y_min){

    const int points_size = _cloud.cloud_ptr->points.size();
    std::vector<std::vector<double> > v1(points_size,vector<double>(2)); 


    double points_xy [points_size][2];
    for(int iter = 0; iter < _cloud.cloud_ptr->points.size(); iter++)
    {

            // points_xy[i][0] = _cloud.cloud_ptr->points[iter].x;
            // points_xy[i][1] = _cloud.cloud_ptr->points[iter].y;
            
            int i = int((_cloud.cloud_ptr->points[iter].x - x_min) / map_resolution);
            if(i < 0 || i >= msg.info.width) continue;

            int j = int((_cloud.cloud_ptr->points[iter].y - y_min) / map_resolution);
            if(j < 0 || j >= msg.info.height - 1) continue;
            
            msg.data[i + j * msg.info.width] = 100;

          
    }

    //这个函数 在 global_local_map 只会调用一次
    global_msg = msg;


 
}
    


void Pointcloud2dProcess::local_map_process(const CloudData& _cloud,nav_msgs::OccupancyGrid& msg,const double& x_min,const double& y_min){
    for(int iter = 0; iter < _cloud.cloud_ptr->points.size(); iter++)
    {
        for(int x = -inf_step ; x <= inf_step; x ++ )
        {
            for(int y = -inf_step ; y <= inf_step; y ++ )
            {
                double inf_x = _cloud.cloud_ptr->points[iter].x + x * _resolution;
                double inf_y = _cloud.cloud_ptr->points[iter].y + y * _resolution;
                int i = int((inf_x - x_min) / map_resolution);
                if(i < 0 || i >= msg.info.width) continue;

                int j = int((inf_y - y_min) / map_resolution);
                if(j < 0 || j >= msg.info.height - 1) continue;
                

                if(x == -inf_step || y == -inf_step || x == inf_step || y == inf_step){
                    msg.data[i + j * msg.info.width] = 81;
                }
                else{
                    msg.data[i + j * msg.info.width] = 99;

                }
            }
        }

      // msg.data[i + j * msg.info.width] = 100;
      //msg.data[i + j * msg.info.width] = int(255 * (_cloud.cloud_ptr->points[iter].z * k_line + b_line)) % 255;
    }

    for(int iter = 0; iter < _cloud.cloud_ptr->points.size(); iter++)
    {
      
            int i = int((_cloud.cloud_ptr->points[iter].x - x_min) / map_resolution);
            if(i < 0 || i >= msg.info.width) continue;

            int j = int((_cloud.cloud_ptr->points[iter].y - y_min) / map_resolution);
            if(j < 0 || j >= msg.info.height - 1) continue;
            
            msg.data[i + j * msg.info.width] = 100;

          
    }
}



void Pointcloud2dProcess::detectObjectsOnCloud(const CloudData& cloud, const CloudData &cloud_filtered)
{
	if (cloud.cloud_ptr->size() > 0)
	{
		//------------------------------------------PCL分割框架--------------------------------------------------------   
		//创建分割时所需要的模型系数对象，coefficients及存储内点的点索引集合对象inliers
		pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
		pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
		// 创建分割对象
		pcl::SACSegmentation<CloudData::POINT> seg;
		// 可选择配置，设置模型系数需要优化
		seg.setOptimizeCoefficients(true);
		// 必要的配置，设置分割的模型类型，所用的随机参数估计方法，距离阀值，输入点云
		seg.setModelType(pcl::SACMODEL_PLANE);//设置模型类型
		seg.setMethodType(pcl::SAC_RANSAC);//设置随机采样一致性方法类型
		seg.setMaxIterations(500);//最大迭代次数
		seg.setDistanceThreshold(1);//设定距离阀值，距离阀值决定了点被认为是局内点是必须满足的条件
		seg.setInputCloud(cloud.cloud_ptr);//输入所需要分割的点云对象
		//引发分割实现，存储分割结果到点几何inliers及存储平面模型的系数coefficients
		seg.segment(*inliers, *coefficients);
		//---------------------------------------------------------------------------------------------------------------
		if (inliers->indices.size() == 0)
		{
			cout << "error! Could not found any inliers!" << endl;
		}
		// extract ground
		// 从点云中抽取分割的处在平面上的点集
		pcl::ExtractIndices<CloudData::POINT> extractor;//点提取对象
		extractor.setInputCloud(cloud.cloud_ptr);
		extractor.setIndices(inliers);
		extractor.setNegative(true);
		extractor.filter(*cloud_filtered.cloud_ptr);
		// vise-versa, remove the ground not just extract the ground
		// just setNegative to be true
		cout << "filter done." << endl;
	}
	else
	{
		cout << "no data!" << endl;
	}
}

void Pointcloud2dProcess::removeClosePoints(double min_distance, const CloudData &cloud_in,
                            const CloudData & cloud_out){


    pcl::ExtractIndices<CloudData::POINT> cliper;
 
    cliper.setInputCloud(cloud_in.cloud_ptr);
    pcl::PointIndices indices;
  #pragma omp for
    for (size_t i = 0; i < cloud_in.cloud_ptr->points.size(); i++)
    {
        double distance = sqrt(cloud_in.cloud_ptr->points[i].x*cloud_in.cloud_ptr->points[i].x + cloud_in.cloud_ptr->points[i].y*cloud_in.cloud_ptr->points[i].y);

        if (distance < min_distance)
        {
            indices.indices.push_back(i);
        }
    }
    cliper.setIndices(boost::make_shared<pcl::PointIndices>(indices));
    cliper.setNegative(true); //ture to remove the indices
    cliper.filter(*cloud_out.cloud_ptr);
}

void Pointcloud2dProcess::Pointcloud_to_2d_grid(const CloudData& _cloud, nav_msgs::OccupancyGrid& msg,const std::string &map_type)
{


  if(map_type == "local msg in global msg process"){
    msg = global_msg;

    CloudData filter_cloud;
    detectObjectsOnCloud(_cloud,filter_cloud);
    removeClosePoints(0.5,filter_cloud,filter_cloud);
    //msg.info.origin.position.x 代表 x_min msg.info.origin.position.y 代表 y_min
    local_map_process(filter_cloud,msg, msg.info.origin.position.x, msg.info.origin.position.y);

    return ;
  }

  inf_step   = round(_cloud_margin * _inv_resolution); 

  msg.header.seq = 0;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = "map";

  msg.info.map_load_time = ros::Time::now();
  msg.info.resolution = map_resolution;

  double x_min, x_max, y_min, y_max;
  double z_max_grey_rate = 0.05;
  double z_min_grey_rate = 0.95;
  double k_line = (z_max_grey_rate - z_min_grey_rate) / (max_z - min_z);
  double b_line = (max_z * z_min_grey_rate - min_z * z_max_grey_rate) / (max_z - min_z);

  if(_cloud.cloud_ptr->points.empty())
  {
    ROS_WARN("pcd is empty!\n");
    return;
  }

  for(int i = 0; i < _cloud.cloud_ptr->points.size() - 1; i++)
  {
    if(i == 0)
    {
      x_min = x_max = _cloud.cloud_ptr->points[i].x;
      y_min = y_max = _cloud.cloud_ptr->points[i].y;
    }

    double x = _cloud.cloud_ptr->points[i].x;
    double y = _cloud.cloud_ptr->points[i].y;

    if(x < x_min) x_min = x;
    if(x > x_max) x_max = x;

    if(y < y_min) y_min = y;
    if(y > y_max) y_max = y;
  }

  msg.info.origin.position.x = x_min;
  msg.info.origin.position.y = y_min;
  msg.info.origin.position.z = 0.0;
  msg.info.origin.orientation.x = 0.0;
  msg.info.origin.orientation.y = 0.0;
  msg.info.origin.orientation.z = 0.0;
  msg.info.origin.orientation.w = 1.0;

  msg.info.width = int((x_max - x_min) / map_resolution);
  msg.info.height = int((y_max - y_min) / map_resolution);

  msg.data.resize(msg.info.width * msg.info.height);
  msg.data.assign(msg.info.width * msg.info.height, 0);

  ROS_INFO("data size = %d\n", msg.data.size());



  if(map_type == "global_map"){
    global_map_process(_cloud,msg, x_min, y_min);
    std::cout << " global map ok " << std::endl;
  }
  else if(map_type == "local_map"){
    CloudData filter_cloud;
    detectObjectsOnCloud(_cloud,filter_cloud);
    removeClosePoints(0.5,filter_cloud,filter_cloud);
    local_map_process(filter_cloud,msg, x_min, y_min);

  }


    // msg.data[i + j * msg.info.width] = 100;
//    msg.data[i + j * msg.info.width] = int(255 * (pcd_cloud.cloud_ptr->points[iter].z * k_line + b_line)) % 255;
}





}
 // namespace data_input